/*
 * @Author: luoqi
 * @Date: 2023-05-18 15:35:23
 * @ Modified by: luoqi
 * @ Modified time: 2024-03-14 15:06
 */

#include "qlink.h"

static const uint16_t crc16tab[256] = {
    0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7,
    0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef,
    0x1231,0x0210,0x3273,0x2252,0x52b5,0x4294,0x72f7,0x62d6,
    0x9339,0x8318,0xb37b,0xa35a,0xd3bd,0xc39c,0xf3ff,0xe3de,
    0x2462,0x3443,0x0420,0x1401,0x64e6,0x74c7,0x44a4,0x5485,
    0xa56a,0xb54b,0x8528,0x9509,0xe5ee,0xf5cf,0xc5ac,0xd58d,
    0x3653,0x2672,0x1611,0x0630,0x76d7,0x66f6,0x5695,0x46b4,
    0xb75b,0xa77a,0x9719,0x8738,0xf7df,0xe7fe,0xd79d,0xc7bc,
    0x48c4,0x58e5,0x6886,0x78a7,0x0840,0x1861,0x2802,0x3823,
    0xc9cc,0xd9ed,0xe98e,0xf9af,0x8948,0x9969,0xa90a,0xb92b,
    0x5af5,0x4ad4,0x7ab7,0x6a96,0x1a71,0x0a50,0x3a33,0x2a12,
    0xdbfd,0xcbdc,0xfbbf,0xeb9e,0x9b79,0x8b58,0xbb3b,0xab1a,
    0x6ca6,0x7c87,0x4ce4,0x5cc5,0x2c22,0x3c03,0x0c60,0x1c41,
    0xedae,0xfd8f,0xcdec,0xddcd,0xad2a,0xbd0b,0x8d68,0x9d49,
    0x7e97,0x6eb6,0x5ed5,0x4ef4,0x3e13,0x2e32,0x1e51,0x0e70,
    0xff9f,0xefbe,0xdfdd,0xcffc,0xbf1b,0xaf3a,0x9f59,0x8f78,
    0x9188,0x81a9,0xb1ca,0xa1eb,0xd10c,0xc12d,0xf14e,0xe16f,
    0x1080,0x00a1,0x30c2,0x20e3,0x5004,0x4025,0x7046,0x6067,
    0x83b9,0x9398,0xa3fb,0xb3da,0xc33d,0xd31c,0xe37f,0xf35e,
    0x02b1,0x1290,0x22f3,0x32d2,0x4235,0x5214,0x6277,0x7256,
    0xb5ea,0xa5cb,0x95a8,0x8589,0xf56e,0xe54f,0xd52c,0xc50d,
    0x34e2,0x24c3,0x14a0,0x0481,0x7466,0x6447,0x5424,0x4405,
    0xa7db,0xb7fa,0x8799,0x97b8,0xe75f,0xf77e,0xc71d,0xd73c,
    0x26d3,0x36f2,0x0691,0x16b0,0x6657,0x7676,0x4615,0x5634,
    0xd94c,0xc96d,0xf90e,0xe92f,0x99c8,0x89e9,0xb98a,0xa9ab,
    0x5844,0x4865,0x7806,0x6827,0x18c0,0x08e1,0x3882,0x28a3,
    0xcb7d,0xdb5c,0xeb3f,0xfb1e,0x8bf9,0x9bd8,0xabbb,0xbb9a,
    0x4a75,0x5a54,0x6a37,0x7a16,0x0af1,0x1ad0,0x2ab3,0x3a92,
    0xfd2e,0xed0f,0xdd6c,0xcd4d,0xbdaa,0xad8b,0x9de8,0x8dc9,
    0x7c26,0x6c07,0x5c64,0x4c45,0x3ca2,0x2c83,0x1ce0,0x0cc1,
    0xef1f,0xff3e,0xcf5d,0xdf7c,0xaf9b,0xbfba,0x8fd9,0x9ff8,
    0x6e17,0x7e36,0x4e55,0x5e74,0x2e93,0x3eb2,0x0ed1,0x1ef0
};

static uint16_t _crc16(const uint8_t *buf, int len) {
    int counter;
    uint16_t crc = 0;
    for(counter = 0; counter < len; counter++)
        crc = (crc << 8) ^ crc16tab[((crc >> 8) ^ *buf++) & 0x00FF];
    return crc;
}

int qlink_init(QLink *link, QLinkSend send, QLinkRecv recv, uint8_t *framebuf, uint8_t framebuf_size, uint8_t *rbuf, uint8_t rbuf_size)
{
    link->recvcnt = 0;
    link->rframe.data = framebuf;
    link->rfame_buf_size = framebuf_size;
    link->rbuf = rbuf;
    link->rbuf_size = rbuf_size;
    link->sframe.sof = 0x514c;
    link->sframe.serial = 0;
    link->sframe.size = 0;
    link->sframe.eof = 0x69;

    link->send = send;
    link->recv = recv;
    return 0;
}

int qlink_send(QLink *link, uint8_t type, uint8_t *data, uint8_t size)
{
    link->sframe.serial++;
    link->sframe.data = data;
    link->sframe.size = size;
    link->sframe.type = type;
    uint8_t buf[5] = { link->sframe.sof & 0xff, (link->sframe.sof >> 8) & 0xff, link->sframe.serial, link->sframe.type, link->sframe.size };
    if(size == 0) {
        link->sframe.crc = _crc16(&link->sframe.serial, 3);
    } else {
        link->sframe.crc = _crc16(link->sframe.data, size) + link->sframe.serial + link->sframe.size + link->sframe.type;
    }
    if(link->send(buf, 5) < 0){
        return -1;
    }
    if(size != 0){
        if(link->send(data, size) < 0){
            return -2;
        }
    }
    buf[0] = link->sframe.crc & 0xff;
    buf[1] = (link->sframe.crc >> 8) & 0xff;
    buf[2] = link->sframe.eof;
    buf[3] = 0x00;
    buf[4] = 0x00;
    if(link->send(buf, 3) < 0){
        return -3;
    }
    return link->sframe.size + 8;
}

int qlink_recv(QLink *link, uint8_t size)
{
    if(link->recv(link->rbuf, size) != size){
        return -4;
    }
    return qlink_unpack(link, link->rbuf, size);
}

int qlink_pack(QLinkFrame *frame, uint8_t type, uint8_t *data, uint8_t size)
{
    frame->sof = 0x514c;
    frame->serial++;
    frame->data = data;
    frame->size = size;
    frame->type = type;
    if(size == 0) {
        frame->crc = _crc16(&frame->serial, 3);
    } else {
        frame->crc = _crc16(frame->data, size) + frame->serial + frame->size + frame->type;
    }
    frame->eof = 0x69;
    return 0;
}

int qlink_unpack(QLink *link, uint8_t *input, int size)
{
    for(int i = 0; i < size; i++) {
        if((link->recvcnt == 0) && (input[i] == 0x4c)) {
            link->rframe.sof |= (uint16_t)input[i];
            link->recvcnt++;
        } else if((link->recvcnt == 1) && (input[i] == 0x51)) {
            link->rframe.sof |= ((uint16_t)input[i] << 8);
            link->recvcnt++;
        } else if(link->recvcnt == 2) {
            link->rframe.serial = input[i];
            link->recvcnt++;
        } else if(link->recvcnt == 3) {
            link->rframe.type = input[i];
            link->recvcnt++;
        } else if(link->recvcnt == 4) {
            link->rframe.size = input[i];
            if(link->rframe.size > link->rfame_buf_size){
                return -3;
            }
            link->recvcnt++;
        } else if((link->recvcnt > 4) && (link->recvcnt <= (link->rframe.size + 4))) {
            link->rframe.data[link->recvcnt - 5] = input[i];
            link->recvcnt++;
        } else if(link->recvcnt == (link->rframe.size + 5)) {
            link->rframe.crc = (uint16_t)input[i];
            link->recvcnt++;
        } else if(link->recvcnt == (link->rframe.size + 6)) {
            link->rframe.crc |= (uint16_t)input[i] << 8;
            link->recvcnt++;
        } else if(link->recvcnt == (link->rframe.size + 7)) {
            link->rframe.eof = input[i];
            if(link->rframe.eof != 0x69) {
                link->recvcnt = 0;
                return -1;
            }
            uint16_t crc = 0;
            if(link->rframe.size == 0) {
                uint8_t buf[3] = { link->rframe.serial, link->rframe.type, link->rframe.size };
                crc = _crc16(buf, 3);
            } else {
                crc = _crc16(link->rframe.data, link->rframe.size) + link->rframe.serial + link->rframe.size + link->rframe.type;
            }
            if(crc != link->rframe.crc) {
                link->recvcnt = 0;
                return -2;
            }
            link->recvcnt = 0;
            return link->rframe.size + 8;
        } else {
            link->recvcnt = 0;
        }
    }
    return 0;
}
